#include "GPS_ATGM336H.h"

#define SYSTEM_SUPPORT_OS        1        //定义系统文件夹是否支持OS
#if SYSTEM_SUPPORT_OS
#include "FreeRTOS.h"
#include "task.h"
#include "timers.h"
#include "queue.h"
#include "event_groups.h"
#include "semphr.h"
#endif
#include "my_include.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>   

#define     ATGM336H_STK_SIZE                 256            //任务堆栈大小

ATGM336_Cmd_TypeDef  ATGM336_CMD = {0};

GPS_type gps_data = {0};             //

QueueHandle_t   *ATGM336H_Receive_Q; 
TaskHandle_t     ATGM336H_TASK_HANDLER;                        //任务句柄


#if  MQTT_INFO_EN        //是否启用MQTT协议通讯
/*****************************************************************************
Function name : format_Position_mqtt_Data
Description: 获取 Position 信息 到 串口透传 服务器
Input:  NONE
Return: NONE 
Notes:
******************************************************************************/
uint16_t format_Position_mqtt_Data( char send[] )
{
    uint16_t pin_id = 0;
    uint16_t lend = 0;
    //确保 定位  模块  在线    
    if(  ATGM336_CMD.Online  == 1)
    {
        lend = 0;
        
        pin_id = (uint16_t)((42<<8)&0xFF01 );    //高8bit 设备类型 低8bit 编号
        
        lend = sprintf(send,"sensor,{\"id\":\"%d\",",pin_id);
        lend += sprintf(send+lend,"\"module\":\"%d\",",42);
        lend += sprintf(send+lend,"\"name\":\"Position\",\"data\":{"); 

        lend += sprintf(send+lend,"\"type\":\"%d\",",1);  // 1百度坐标系
        if( gps_data.BD09Longitude && gps_data.BD09Latitude )
        {
            lend += sprintf(send+lend,"\"lng\":\"%f\",",gps_data.BD09Longitude);  //经度坐标
            
            lend += sprintf(send+lend,"\"lat\":\"%f\"",gps_data.BD09Latitude);  //纬度坐标
        }else{
            lend += sprintf(send+lend,"\"lng\":null,\"lat\":null");  
        } 
        lend += sprintf(send+lend,"} }\r\n");
        
        return lend;
    }
    return 0;
}

#endif  //MQTT_INFO_EN  //是否启用MQTT协议通讯


const double pi = 3.14159265358979324;
const double a =  6378245.0;                    //  a: 卫星椭球坐标投影到平面地图坐标系的投影因子。
const double ee = 0.00669342162296594323;
const double x_pi = 3.14159265358979324 * 3000.0 / 180.0;  


void del_char(char a[],char c)//在字符串中删除某个字符
{
    int i,j;
    for(i=0,j=0; *(a+i)!='\0'; i++)
    {
        if(*(a+i)==c)
            continue;
        else
        {
            *(a+j)=*(a+i);
            j++;
        }
    }
    *(a+j)='\0';
}
static bool outOfChina(double lat, double lon) 
{ 
    if (lon < 72.004 || lon > 137.8347)
        return true; 
    if (lat < 0.8293 || lat > 55.8271)
        return true; 
    return false;
} 
static double transformLat(double x, double y)
{ 
    double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(abs(x));
    ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0; 
    ret += (20.0 * sin(y * pi) + 40.0 * sin(y / 3.0 * pi)) * 2.0 / 3.0; 
    ret += (160.0 * sin(y / 12.0 * pi) + 320 * sin(y * pi / 30.0)) * 2.0 / 3.0; 
    return ret;
} 
static double transformLon(double x, double y)
{ 
    double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(abs(x)); 
    ret += (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0; 
    ret += (20.0 * sin(x * pi) + 40.0 * sin(x / 3.0 * pi)) * 2.0 / 3.0;
    ret += (150.0 * sin(x / 12.0 * pi) + 300.0 * sin(x / 30.0 * pi)) * 2.0 / 3.0; 
    return ret;
} 
/*
参数
wgLat:WGS-84纬度wgLon:WGS-84经度
返回值：
mgLat：GCJ-02纬度mgLon：GCJ-02经度
*/
void gps_transform( double wgLat, double wgLon, double mgLat, double mgLon) 
{ 
    if (outOfChina(wgLat, wgLon)) 
    { 
        mgLat = wgLat;
        mgLon = wgLon;
        return; 
    } 
    double dLat = transformLat(wgLon - 105.0, wgLat - 35.0); 
    double dLon = transformLon(wgLon - 105.0, wgLat - 35.0); 
    double radLat = wgLat / 180.0 * pi; double magic = sin(radLat);
    magic = 1 - ee * magic * magic; double sqrtMagic = sqrt(magic);
    dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
    dLon = (dLon * 180.0) / (a / sqrtMagic * cos(radLat) * pi);
    mgLat = wgLat + dLat; mgLon = wgLon + dLon;
}
 

//将 GCJ-02 坐标转换成 BD-09 坐标
void bd_encrypt(double gg_lat, double gg_lon, double bd_lat, double bd_lon)  
{  
    double x = gg_lon, y = gg_lat;  
    double z = sqrt(x * x + y * y) + 0.00002 * sin(y * x_pi);  
    double theta = atan2(y, x) + 0.000003 * cos(x * x_pi);  
    bd_lon = z * cos(theta) + 0.0065;  
    bd_lat = z * sin(theta) + 0.006;  
}  
  
void bd_decrypt(double bd_lat, double bd_lon, double gg_lat, double gg_lon)  
{  
    double x = bd_lon - 0.0065, y = bd_lat - 0.006;  
    double z = sqrt(x * x + y * y) - 0.00002 * sin(y * x_pi);  
    double theta = atan2(y, x) - 0.000003 * cos(x * x_pi);  
    gg_lon = z * cos(theta);  
    gg_lat = z * sin(theta);  
} 


// 地球坐标系(WGS-84)转火星坐标系(GCJ) 转 火星坐标转百度坐标：
void transformWGS84ToGCJ02ToBD09(GPS_type *pGPS )
{
 
    double wgLat=pGPS->trueLatitude;
    double wgLon=pGPS->trueLongitude;
 
    double mars_pointlat;
    double mars_pointlng;

    if (outOfChina(wgLat,wgLon)) {
        mars_pointlat = wgLat;
        mars_pointlng = wgLon;
        return;
    }
    double dLat = transformLat(wgLon - 105.0, wgLat - 35.0);
    double dLon = transformLon(wgLon - 105.0, wgLat - 35.0);
    double radLat = (wgLat / 180.0) * pi;
    double magic = sin(radLat);
    magic = 1 - ee * magic * magic;
    double sqrtMagic = sqrt(magic);
    dLat = (dLat * 180.0) / (((a * (1 - ee)) / (magic * sqrtMagic)) * pi);
    dLon = (dLon * 180.0) / ((a / sqrtMagic) * cos(radLat) * pi);
    mars_pointlat = wgLat + dLat;
    mars_pointlng = wgLon + dLon;
    //=============================================
 
    double x2 = mars_pointlng * mars_pointlng;
    double y2 = mars_pointlat * mars_pointlat;
    double z = sqrt(x2 + y2) + 0.00002 * sin(mars_pointlat * x_pi);
    double theta = atan2(mars_pointlat, mars_pointlng) + 0.000003 * cos(mars_pointlng * x_pi);
 
    pGPS->BD09Latitude  = z * sin(theta) + 0.006;
    pGPS->BD09Longitude = z * cos(theta) + 0.0065;
 
}
/*****************************************************************************
Function name : GPS_GNGGA_TO_WGS84
Description: 定位坐标字符串数据转成 wgs84 坐标
Input: NONE: 接收到的串口数据。/ 数据长度
Return: NONE  
Modify Date: 2020.9.3
Author : Lu
Notes:
******************************************************************************/
double GPS_GNGGA_TO_WGS84(char *buff, uint16_t lend)
{
    /*****     
    我们需要把它转换成度分秒的格式，计算方法：
    如接收到的纬度是：4546.40891
    　　4546.40891/100=45.4640891可以直接读出45度, 
        4546.40891–45*100=46.40891, 可以直接读出46分
        46.40891–46 =0.40891*60=24.5346读出24秒, 
    所以纬度是：45度46分24秒。
    *****/
    
    
    uint16_t gps_integer = 0;
    double gps_decimal = 0;
    double gps_deci = 0.0 ,gps_deg = 0.0,gps_s = 0.0;
    char *subString=NULL;
    if( (subString = strstr(buff,".")))
    {
        gps_integer = atoi(buff);                       //整数部分
        gps_decimal = atof(buff);
        gps_decimal = gps_decimal - gps_integer;

        gps_s  =  (gps_decimal)*60;                    //秒
        gps_deg = (double)(gps_integer%100) ;         //分
        gps_deci = (double)(gps_integer/100) + (double)(gps_deg/100) + (double)(gps_s/10000);
        return  gps_deci ;
    } 
    return 0;
}

/*****************************************************************************
Function name : GPS_GNGGA_TO_WGS84
Description: 定位坐标字符串数据转成 wgs84 坐标
Input: NONE: 接收到的串口数据。/ 数据长度
Return: NONE  
Modify Date: 2020.9.3
Author : Lu
Notes:
******************************************************************************/
double GPS_GNGGA_TO_Degree(char *buff, uint16_t lend)
{
/****
    比如这帧数据里面，“3413.64266”这个数是34度加13.64266分，
    分到度是60进制，因此要把分转换成度需要把分除以60，
    意思是13.64266分等于13.64266 / 60 = 0.227378度，
    那么3413.64266这个数就是34.227378度。
    同理经度也是一样。
*****/
    double d_lon = 0;
    double d_tmp1 = 0;
    double d_tmp2 = 0;
    char *subString=NULL;
    if( (subString = strstr(buff,".")))
    {
        /* 将度分格式转换为度格式 */
        d_lon = atof(buff);
        d_lon /= 100;
        d_tmp1 = floor(d_lon);
        d_tmp2 = d_lon - d_tmp1;
        d_tmp2 *= 100;
        d_tmp2 /= 60;
        d_lon = d_tmp1 + d_tmp2;
        return d_lon;
    } 
    return 0;
}
 //----------------------ATGM336-GPS --------------------------
/*****************************************************************************
Function name : Get_GPS_ATGM336
Description: 获取定位坐标数据 
Input: NONE: 接收到的串口数据。/ 数据长度
Return: NONE  
Modify Date: 2020.9.3
Author : Lu
Notes:
******************************************************************************/
double longitude = 0.0 ,latitude = 0.0;
double longitude_copy = 0.0 ,latitude_copy = 0.0;
double gps_fabs = 0.0;

uint8_t Get_GPS_ATGM336(uint8_t *buff, uint16_t lend)
{
    uint8_t ret = 0;
    usefullBuffer[0] = 0;
    
    char gps_longitude_buff[40]={0}, gps_latitude_buff[40]={0};   //临时缓存经纬度

    //坐标数据解析
    char *subString=NULL,*subStringNext=NULL,i=0; 
#if GPS_EN
//    char *data=strstr((char*)buff,"$GNGGA");      //GPS定位信息
#endif
    
#if BD_EN
    char *data=strstr((char*)buff,"$BDGGA");       //北斗定位信息
#endif 
    
    if(data!=NULL&&(data-(char*)buff)<(lend-100))
    {   
        for (i = 0 ; i <= 6 ; i++)          
        {
            if (i == 0)
            {
           #if BD_EN     
                subString = strstr((char*)buff, "$BDGGA");          //北斗定位信息
           #endif
                
           #if GPS_EN
                subString = strstr((char*)buff, "$GNGGA");        //GPS定位信息
           #endif
                
                subString = strstr(subString, ",");
            }
            else{
                subString++;
                if ((subStringNext = strstr(subString, ",")) != NULL)
                {
                    //+QGNSSRD: $GNRMC,030617.000,A,2310.8455,N,11323.9394,E,0.24,209.90,121020,,,D*7B
                    //+MGNS;SLOC: 061116.00,3028.1249N,11424.7311E,3.46,52.1,3,,5.589,3.018,140720,7
                    switch(i)
                    {
                        //case 1:memcpy(Save_Data.UTCTime, subString, subStringNext - subString);break;    //获取UTC时间
                        case 2:
                            memset(gps_latitude_buff, 0, sizeof(gps_latitude_buff));
                            memcpy(gps_latitude_buff, subString, subStringNext - subString);
                            if( gps_latitude_buff[0] !='0'&& gps_latitude_buff[0] != 0 )
                            {
                                latitude = atof(gps_latitude_buff);
                                if ( latitude ) 
                                {
                                    gps_fabs = fabs( latitude - latitude_copy ) ;
                                    if( gps_fabs >= 15 )  //定位差值大于15 
                                    {
                                         usefullBuffer[0]='A'; 
                                         del_char(gps_latitude_buff,'N');
                                    }
                                }
                            }
                        break;    //获取纬度信息
                        //case 3:memcpy(Save_Data.N_S, subString, subStringNext - subString);break;    //获取N/S
                        case 4:
                            memset(gps_longitude_buff, 0, sizeof(gps_longitude_buff));
                            memcpy(gps_longitude_buff, subString, subStringNext - subString);
                            if( gps_longitude_buff[0] !='0'&& gps_longitude_buff[0] != 0 )
                            {
                                longitude = atof(gps_longitude_buff);
                                if ( longitude ) 
                                {
                                    gps_fabs = fabs( longitude - longitude_copy ) ;
                                    if( gps_fabs >= 15 )  //定位差值大于15 
                                    {
                                        usefullBuffer[0]='A'; 
                                        del_char(gps_longitude_buff,'E');
                                    }
                                }
                            }
                        break;    //获取经度信息
                        //case 5:memcpy(Save_Data.E_W, subString, subStringNext - subString);break;    //获取E/W    
                        default:break;
                    }
                    subString = subStringNext;
                }
            }
        }
        if( usefullBuffer[0]=='A' && (gps_longitude_buff[0]!='\0'&& gps_latitude_buff[0]!='\0') )
        {      
            /**********************将GPS坐标转换成百度坐标***************************/
            gps_data.trueLatitude = GPS_GNGGA_TO_Degree(gps_latitude_buff,40);
            gps_data.trueLongitude = GPS_GNGGA_TO_Degree(gps_longitude_buff,40);
//            gps_data.trueLatitude = GPS_GNGGA_TO_WGS84(gps_latitude_buff,40);
//            gps_data.trueLongitude = GPS_GNGGA_TO_WGS84(gps_longitude_buff,40);
//            gps_data.trueLatitude = atof(gps_latitude_buff);
//            gps_data.trueLongitude = atof(gps_longitude_buff);
            transformWGS84ToGCJ02ToBD09(&gps_data);
            /******************将百度坐标转换成字符串保存到内存中*********************/
            sprintf(gps_longitude_buff,"%f",gps_data.BD09Longitude);
            sprintf(gps_latitude_buff,"%f",gps_data.BD09Latitude);
            
            if(gps_latitude_buff[0] !='\0' && gps_longitude_buff[0] !='\0')
            {
                //保存定位到内存中
                flash_update(BT_GPSLON_ADDR,(u8*)gps_longitude_buff,BT_GPSLON_SIZE);
                flash_update(BT_GPSLAT_ADDR,(u8*)gps_latitude_buff,BT_GPSLAT_SIZE);
                
                nm_conf_update_falg = 3;        //主动更新定位信息
                
//                uint16_t send_lend = 0;
//                char *send_buff = (char*)&flash_update_buf;
//                memset(flash_update_buf,0,FLASH_PAGE_SIZE);
//                send_lend = format_Position_mqtt_Data(send_buff);
//                mqtt_msg_send(send_buff,send_lend,MQTT_4G);
                
                ret = 1;
            }
             ret = 0;
        }
        else  //无效位V，或者+QGNSSRD: .3,M,,*51
        {
            ret = 0;
        }
        longitude_copy = longitude ;
        latitude_copy = latitude;
    }
    return ret;
}





uint8_t ATGM336_send_cmd(char *cmd,char *ack,uint16_t waittime)
{
    
    if(ack==NULL&&cmd==NULL) return 0;
    
    ATGM336_CMD.tbuf_count  = sprintf((char*)ATGM336_CMD.tbuf,"%s\r\n",cmd);    
 
    ATGM336_Send_data( ATGM336_CMD.tbuf );      //发送命令 
    
    while(--waittime)               //等待倒计时
    { 
        delay_ms(10);
        
        if(ATGM336_CMD.rbuf_count)//接收到期待的应答结果
        {
            if( strstr((char*)ATGM336_CMD.rbuf,ack)) 
            {
                return 1;  //得到有效数据
            }
            ATGM336H_rbuf_clear();  //清空接收缓存
        } 
    }
    
    return 0;
} 
/*=====================================================
 * Function : ATGM336H_Init
 * Describe : ATGM336H 芯片 事件初始化
 * Input    : none
 * Output   : none
 * Return   : none
=====================================================*/
void ATGM336H_Init(void)
{
    ATGM336_init(9600);    //串口初始化
    
    #ifdef STM32
    TIM7_Int_Init(9999,71);        //10ms中断
    #elif  GD32
    TIM7_Int_Init(9999,107);        //10ms中断
    #endif    
    
    memset(&gps_data,0,sizeof(gps_data));
    memset(&ATGM336_CMD,0,sizeof(ATGM336_CMD));
}
//
void ATGM336H_task(void)
{
    uint8_t res = 0,ATGM336H_rec = 0;

    
    while(1)
    {
        
        switch( ATGM336_CMD.set_ID ){
            case 0:{       //配置工作系统。
    //        $PCAS04,3*1A 北斗和 GPS 双模
    //        $PCAS04,1*18 单 GPS 工作模式
    //        $PCAS04,2*1B 单北斗工作模式
              res = ATGM336_send_cmd("$PCAS04,2*1B","OK",100);  
              ATGM336_CMD.set_ID = (res?1:0);  
            }break;
            case 1:{       //保存配置信息
                //将当前配置信息保存到 FLASH 中，即使接收机完全断电，FLASH 中的信息不丢失。
                res = ATGM336_send_cmd( "$PCAS00*01","OK",100 );  
                ATGM336_CMD.set_ID = (res?2:1);  
            }break;
            case 2:

                //配置参数完成 定时等待芯片返回定位数据
                //等待接收返回的数据
                res = xQueueReceive(ATGM336H_Receive_Q,&ATGM336H_rec,portMAX_DELAY);
                
                if( res )  //收到数据
                {   
                    ATGM336_CMD.Online = 1; //模块在线
                    Get_GPS_ATGM336(ATGM336_CMD.rbuf,ATGM336_CMD.rbuf_count);
                }
                break;
        }
        ATGM336H_rbuf_clear();         //清空接收缓存
        delay_ms(500);
    }
}
/*****************************************************************************
Function name : ATGM336H_recive_char
Description: ATGM336H_接收数据
Input: ch
Return: NONE 
Modify Date: 2021.7.1
Author : Lu
Notes:
******************************************************************************/
void ATGM336H_recive_char(uint8_t ch)
{
    if(ATGM336_CMD.rbuf_count < ATGM336_RBUF_SIZE)
    {
       ATGM336_CMD.rbuf[ATGM336_CMD.rbuf_count] = ch;
       ATGM336_CMD.rbuf_count++;
    }
    
}

void ATGM336H_queue_send(void)
{    
    if(ATGM336_CMD.rbuf_count > 0 ){
        portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; 
        xQueueSendFromISR(ATGM336H_Receive_Q, &ATGM336_CMD.rbuf_count, &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}
/*****************************************************************************
Function name : ATGM336H_rbuf_clear
Description: 清空ATGM336H接收数据
Input: NONE    
Return: NONE 
Modify Date: 2021.7.1
Author : Lu
Notes:
******************************************************************************/
void ATGM336H_rbuf_clear(void)
{
    memset(&ATGM336_CMD.rbuf,0,ATGM336_RBUF_SIZE);
    ATGM336_CMD.rbuf_count = 0;
}
/*****************************************************************************
Function name : ATGM336H_task_create
Description: 创建 ATGM336H 任务
Input: NONE 
Return: NONE  
Modify Date: 2020.9.3
Author : Lu
Notes:
******************************************************************************/
void ATGM336H_task_create(u8 prio)
{
    
    ATGM336H_Receive_Q = xQueueCreate( ATGM336_RBUF_SIZE, sizeof(u8));   //创建接收消息队列
    
    ATGM336H_Init();
    //发送数据任务
    xTaskCreate((TaskFunction_t) ATGM336H_task,
              (char *)"mqtt_task", 
              (uint16_t)ATGM336H_STK_SIZE,
              (void *) NULL, 
              (UBaseType_t) prio, 
              (TaskHandle_t *)ATGM336H_TASK_HANDLER);
}

